function data = SK6_Get_Sensor_Data( ID , num_samples )
% SHAKE SK6 Matlab Driver v1.00, (c) 2010 SAMH Engineering Services
%
% function data = SK6_Get_Sensor_Data( ID , num_samples )
% Get <num_samples> of <ID> sensor data from the receive data buffer
% The data is returned in data which is a matrix of <num_samples> rows
% and up to 4 columns of data depending on how many axes of data the sensor has.
% The 5th column always contains the packet counter.
% Thus the size of the returned matrix is <num_samples> rows by 5 columns
% If <num_samples> is greater than the number of samples available in
% the receive buffer then data will return as a scalar with value false
% Please use SK6_Data_Samples_Available( ID ) to determine the number
% of samples available to read before calling this function.

global BuffLength				% number of samples per sensor in the circular buffer
global BuffData						% circular buffer that contains the parsed data
global BuffOut		% points to the position where data is to be read from
global BuffItems				% holds the number of items in the buffer
global BuffWidth				% number of channels per sensor ( max is 5 for quaternions (4) plus count
% (1). The index of the count variable will always be 5

sensor = int32( ID );

% return immediately with false if the number of samples to read is zero
if num_samples == 0
  data = false;
  return
end

% only transfer the data if there is enough samples in the buffer
if BuffItems( sensor ) >= num_samples
  
  data = int32( zeros ( num_samples , BuffWidth ) );
  
  % calculate the number of samples that are available between the
  % current output pointer and the end of the buffer
  samples_at_end_of_buffer = BuffLength - BuffOut( sensor ) + 1;
  
  if samples_at_end_of_buffer >= num_samples
    %transfer can be done in one block
    data( 1 : num_samples , 1:BuffWidth ) =...
      BuffData( sensor , BuffOut( sensor ):BuffOut( sensor ) + num_samples - 1, 1:BuffWidth );
    % update the buffer output index
    BuffOut( sensor ) = BuffOut( sensor ) + num_samples;
  else
    % transfer the first block ( the pre-wrap part )
    data( 1 : samples_at_end_of_buffer , 1:BuffWidth ) =...
      BuffData( sensor , BuffOut( sensor ):BuffLength , 1:BuffWidth );
    % calculate the number of samples at the start of the buffer
    samples_at_start_of_buffer = num_samples - samples_at_end_of_buffer;
    % transfer the second block ( the post-wrap part )
    data( samples_at_end_of_buffer + 1 : num_samples , 1:BuffWidth ) =...
      BuffData( sensor , 1 : samples_at_start_of_buffer , 1:BuffWidth );
    % update the buffer output index
    BuffOut( sensor ) = samples_at_start_of_buffer + 1;
  end
  %update the number of items in the buffer
  BuffItems( sensor ) = BuffItems( sensor ) - num_samples;
else
  data = false;
end

end
